#include "IndList.h"

IndList::IndList() {
    //this->act = 'P';
}

void IndList::loadFromFile(std::string fileName) {
    Individuals.clear();

    cv::FileStorage fs(fileName, cv::FileStorage::READ);

    cv::FileNode features = fs["Individuals"];
    cv::FileNodeIterator it = features.begin(), it_end = features.end();




    for (; it != it_end; ++it) {
        std::string name;
        cv::Mat meanVector;
        cv::Mat iCovarMat;

        (*it)["name"] >> name;
        //std::cout << name;
        (*it)["meanVector"] >> meanVector;
        (*it)["iCovarMat"] >> iCovarMat;

        addInd(name, meanVector, iCovarMat);
    }
    fs.release();
}

void IndList::saveToFile(std::string fileName) {
    int size = (int) Individuals.size();
    cv::FileStorage fs(fileName, cv::FileStorage::WRITE);
    fs << "Individuals" << "[";
    for (int i = 0; i < size; i++) {
        fs << "{:" << "name" << Individuals.at(i).name;
        fs << "meanVector" << Individuals.at(i).meanVector;
        fs << "iCovarMat" << Individuals.at(i).iCovarMat;
        fs << "}";
    }
    fs << "]";

    fs.release();
}

void IndList::addInd(std::string name, cv::Mat meanVector, cv::Mat iCovarMat) {
    Individual ind(name, meanVector, iCovarMat);
    Individuals.push_back(ind);
}

void IndList::collectSamples(char &act, float d[15][15], int vectorDimension, int bonesIndex[2][15], int nTimes) {
    static int countNTimes = 0;
    static std::vector< std::vector<float> > samples;


    if (countNTimes < nTimes) {

        if (act == 'R' || act == 'r') {
            std::vector<float> sample(vectorDimension);
            for (int i = 0; i < vectorDimension; i++) {
                sample.at(i) = d[bonesIndex[0][i]][bonesIndex[1][i]];
                //std::cout<< d[bonesIndex[0][i]][bonesIndex[1][i]] << std::endl;

            }

            /*
            for (int j = 0; j < sample.size(); j++) {
                std::cout << " " << sample.at(j);
            }*/
            samples.push_back(sample);
            //std::cout << std::endl;

            //std::cout << "sample :  " << sample.size() << std::endl;
            //std::cout << countNTimes << std::endl;
            //std::cout << "SampleS :  " << samples.size() << std::endl;
            sample.clear();
            countNTimes++;
        }
    } else if (countNTimes == nTimes) {
        countNTimes = 0;
        act = 'P';

        float arr[nTimes][vectorDimension];

        for (int i = 0; i < nTimes; i++) {
            for (int j = 0; j < vectorDimension; j++) {
                arr[i][j] = samples.at(i).at(j);
            }
        }

        Mat samplesMat(nTimes, 15, CV_32FC1, (void*) arr);
        Mat covarMat(15, 15, CV_32FC1);
        Mat meanVector(1, 15, CV_32FC1);
        Mat iCovarMat(15, 15, CV_32FC1);

        Stat st;
        st.covarMatrix(samplesMat, covarMat, meanVector);
        st.invertMatrix(covarMat, iCovarMat);

        //std::cout << iCovarMat << std::endl;
        //std::cout << meanVector << std::endl;

        string name;
        std::cout << "Type in a name: " << std::endl;
        std::cin >> name;

        addInd(name, meanVector, iCovarMat);


        samples.clear();
    }
}


std::vector<String> IndList::mahal(float distances[15][15], int bonesIndex[2][15]) {
    //preenche vetor com valores distancia do individuo atual
    float currentVector[15];
    for (int i = 0; i < 15; i++) {
        currentVector[i] = distances[bonesIndex[0][i]][bonesIndex[1][i]];
    }
    //calcula a distancia de mahalanobis pra cada individuo com relação ao atual e coloca num vetor de pares de individuo/par
    Mat currentVectorMat(1, 15, CV_32FC1, (void*) currentVector);
    int size = Individuals.size();
    std::vector< std::pair<Individual*, float> > pairs;
    for (int i = 0; i < size; i++) {
        //float mahaln = 1;
        float mahaln = (float) Mahalanobis(currentVectorMat, Individuals.at(i).meanVector, Individuals.at(i).iCovarMat);
        std::pair<Individual*, float> p;
        p.first = &Individuals.at(i);
        p.second = mahaln;
        pairs.push_back(p);
    }

    //ordena por insertion sort
    //verificar se "size - 1" ou "size"
    for (int i = 1; i < size; i++) {
        std::pair<Individual*, float> item = pairs.at(i);
        int iHole = i;

        while (iHole > 0 && pairs.at(iHole - 1).second > item.second) {
            pairs.at(iHole) = pairs.at(iHole - 1);
            iHole = iHole - 1;
        }
        pairs.at(iHole) = item;
    }

    //imprime na tela as distancias de mahalanobis de cada individuo com relação ao atual sendo visto
    std::ostringstream textStream;

    std::vector<String> stringVector;


    for (int j = 0; j < size; j++) {

        textStream << pairs.at(j).first->name << "    " << pairs.at(j).second;
        stringVector.push_back(textStream.str());
        textStream.str("");

        //std::cout << "mahal( THIS , " << pairs.at(j).first->name << " ) = " << pairs.at(j).second << std::endl;

    }
    return stringVector;

}


/*void IndList::sortedMahal(float distances[15][15], int bonesIndex[2][15]) {
    float currentVector[15];
    for (int i = 0; i < 15; i++) {
        currentVector[i] = (float) distances[bonesIndex[0][i]][bonesIndex[1][i]];
    }

    std::vector<SimpleIndividual> sIndividuals;
    Mat currentVectorMat(1, 15, CV_32FC1, (void*) currentVector);
    for (int j = 0; j < Individuals.size(); j++) {

        float mahaln = Mahalanobis(currentVectorMat, Individuals.at(j).meanVector, Individuals.at(j).iCovarMat);

        std::cout << "mahal( THIS , " << Individuals.at(j).name << " ) = " << mahaln << std::endl;
        SimpleIndividual sI(Individuals.at(j).name,mahaln);
        sIndividuals.push_back(sI);
    }


}
 */